#ifndef URCONTROLDEPTHCAMERAA_HPP
#define URCONTROLDEPTHCAMERAA_HPP
#include "urcontroldepthcamera.hpp"

//for RG2 setup

class URControlDepthCameraA : public URControlDepthCamera
{
public:
    static cv::Matx22d RM2(double rad)
    {
        return cv::Matx22d(cos(rad),-sin(rad),sin(rad),cos(rad));
    }

    static cv::Vec6d CalibrateDepthCameraRG2()
    {
        cv::Vec2d offxy=RM2(-CV_PI/4)*cv::Vec2d(32,-(18+8+24));

        cv::Vec3d offsetToRG2(offxy[0], offxy[1], 25+20+68);

        std::cout<<"offsetToRG2="<<offsetToRG2<<"\n\n\n\n\n"<<std::flush;

        offsetToRG2=offsetToRG2/1000;

        cv::Vec6d toRG2(offsetToRG2[0], offsetToRG2[1], offsetToRG2[2], 0, 0, 0);

        cv::Vec6d RG2ToHead(0,0,0,-CV_PI/2,0,0);

        cv::Vec6d RG2ToHead2(0,0,0,0,0,CV_PI*0.75);

        return URControlOffset::ComposeTransform(URControlOffset::ComposeTransform(RG2ToHead, RG2ToHead2),toRG2);
    }

    static bool PointInRotateRect(float x, float y, cv::RotatedRect rect)
    {
        float dx=x-rect.center.x;
        float dy=y-rect.center.y;
        float rad=-rect.angle*CV_PI/180;
        cv::Matx22f R(cos(rad), -sin(rad), sin(rad), cos(rad));
        cv::Vec2f pto=R*cv::Vec2f(dx,dy);
        if(fabs(pto[0])<rect.size.width/2 && fabs(pto[1])<rect.size.height/2)
            return true;
        return false;
    }

    //ensure unit normal vector

    static double GetObjectHeight(const cv::Mat &xyz, cv::RotatedRect objectOnFrame, cv::Vec4f plane)
    {
        if(plane[3]<0)
            return GetObjectHeight(xyz, objectOnFrame, -plane);

        double maxd=-1;

        for(int y=0;y<xyz.rows;y++)
        {
            for(int x=0;x<xyz.cols;x++)
            {
                cv::Vec3f pt=xyz.at<cv::Vec3f>(y,x);

                if(pt[2]!=0 && PointInRotateRect(x, y, objectOnFrame))
                {
                    cv::Vec4f ptn(pt[0],pt[1],pt[2],1);
                    double d=ptn.ddot(plane);
                    if(d>0 && maxd<d)
                    {
                        maxd=d;
                    }
                }
            }
        }

        return maxd;
    }



    static cv::RotatedRect SelectObjectRect(const cv::Mat &src, int x, int y, int thresh = 30)
    {
        cv::Mat src_gray;
        /// Convert image to gray and blur it
        cv::cvtColor(src, src_gray, cv::COLOR_BGR2GRAY);
        cv::blur(src_gray, src_gray, cv::Size(3, 3));


        cv::Mat threshold_output;
        std::vector<std::vector<cv::Point> > contours;
        std::vector<cv::Vec4i> hierarchy;


        /// Detect edges using Threshold
        cv::threshold(src_gray, threshold_output, thresh, 255, cv::THRESH_BINARY);
        /// Find contours
        cv::findContours(threshold_output, contours, hierarchy, cv::RETR_EXTERNAL, cv::CHAIN_APPROX_SIMPLE, cv::Point(0, 0));

        /// Find the rotated rectangles and ellipses for each contour
        std::vector<cv::RotatedRect> minRect(contours.size());
//        vector<RotatedRect> minEllipse(contours.size());

        double mindist = 9999;
        double tempdist = 0;
        int min_i;
        for (size_t i = 0; i < contours.size(); i++)
        {
            if (contours[i].size() > 50)

            {
                minRect[i] = cv::minAreaRect(contours[i]);

                tempdist = abs(minRect[i].center.x - x) + abs(minRect[i].center.y - y);
                //			cout << tempdist << endl;
                if (tempdist < mindist) {
                    min_i = i;
                    mindist = tempdist;
                }

                //			cout << minRect[i].center << endl;
            }

            //         { minEllipse[i] = fitEllipse( contours[i] );}
        }



        cv::Mat m=src.clone();
        cv::Point2f pt[4];
        minRect[min_i].points(pt);


        cv::line(m, cv::Point(pt[0].x,pt[0].y), cv::Point(pt[1].x,pt[1].y), cv::Scalar(0,0,255), 3 );
        cv::line(m, cv::Point(pt[1].x,pt[1].y), cv::Point(pt[2].x,pt[2].y), cv::Scalar(0,255,0), 3 );
        cv::line(m, cv::Point(pt[2].x,pt[2].y), cv::Point(pt[3].x,pt[3].y), cv::Scalar(255,0,0), 3 );
        cv::line(m, cv::Point(pt[3].x,pt[3].y), cv::Point(pt[0].x,pt[0].y), cv::Scalar(255,0,255), 3);

        cv::imshow("m",m);

        return minRect[min_i];
    }



    URControlDepthCameraA()
        : URControlDepthCamera()
    {
        depthCameraToHead=CalibrateDepthCameraRG2();

        std::cout<<"depthCameraToHead="<<depthCameraToHead<<"\n"<<std::flush;
    }



    double GetFingerPosition(cv::Vec3f &fingerPos0, cv::Vec3f &fingerPos1, cv::RotatedRect objectOnFrame, cv::Vec4f plane, double objectHeight, double fingerThick)
    {
        cv::Point2f pt[4];

        objectOnFrame.points(pt);

        cv::Point2f fingerPosOnFrame[2];

        if(cv::norm(pt[0]-pt[1])>cv::norm(pt[0]-pt[3]))
        {
            fingerPosOnFrame[0]=(pt[0]+pt[1])/2;
            fingerPosOnFrame[1]=(pt[2]+pt[3])/2;
        }
        else
        {
            fingerPosOnFrame[0]=(pt[0]+pt[3])/2;
            fingerPosOnFrame[1]=(pt[2]+pt[1])/2;
        }

        std::cout<< "fingerPosOnFrame[0]="<<fingerPosOnFrame[0]<<"\n";
        std::cout<< "fingerPosOnFrame[1]="<<fingerPosOnFrame[1]<<"\n";

        cv::Vec3f fingerRay0=Deproject(cv::Vec2f(fingerPosOnFrame[0].x, fingerPosOnFrame[0].y));
        cv::Vec3f fingerRay1=Deproject(cv::Vec2f(fingerPosOnFrame[1].x, fingerPosOnFrame[1].y));

        std::cout<< "fingerRay0="<<fingerRay0<<"\n";
        std::cout<< "fingerRay1="<<fingerRay1<<"\n";


        cv::Vec3f normal(plane[0],plane[1],plane[2]);//should be unit vector and along to z+ axis


        double plane3top;

        if(plane[3]>0)
        {
            plane3top=plane[3]-objectHeight;
        }
        else
        {
             plane3top=plane[3]+objectHeight;
        }


        cv::Vec3f fingerPosTop0=fingerRay0*(-plane3top/normal.ddot(fingerRay0));
        cv::Vec3f fingerPosTop1=fingerRay1*(-plane3top/normal.ddot(fingerRay1));

        if(objectHeight>fingerThick)
        {
            fingerPos0=fingerPosTop0+objectHeight/2*normal;
            fingerPos1=fingerPosTop1+objectHeight/2*normal;
        }
        else
        {
            fingerPos0=fingerPosTop0+(objectHeight-fingerThick/2)*normal;
            fingerPos1=fingerPosTop1+(objectHeight-fingerThick/2)*normal;
        }


        double width=cv::norm(fingerPos0-fingerPos1);

        std::cout<< "fingerPos0="<<fingerPos0<<"\n";
        std::cout<< "fingerPos1="<<fingerPos1<<"\n";

        std::cout<< "dfn2="<<width<<"\n";

        return width;
    }



    bool PickBox(double &openmm, cv::Vec4d planeAtBase, cv::Vec6d fingerToHead, const cv::Mat &xyz, cv::RotatedRect objectOnFrame, const char *ipUR, const char *ipPC, double a, double v)
    {
         double fingerThickmm=30;


        cv::Vec6d headToBase;
        GetTCPPose(ipUR, ipPC, headToBase[0], headToBase[1], headToBase[2], headToBase[3], headToBase[4], headToBase[5]);

        cv::Vec6d depthCameraToBase=ComposeTransform(depthCameraToHead, headToBase);

        cv::Vec6d baseToDepthCamera=ReverseTransform(depthCameraToBase);

        cv::Vec4d planeAtCamera=HeadToBase(planeAtBase, baseToDepthCamera);

        cv::Vec4f planeAtCameraf(planeAtCamera[0], planeAtCamera[1], planeAtCamera[2], planeAtCamera[3]);



        double objectHeight=GetObjectHeight(xyz, objectOnFrame, planeAtCamera);

        if(objectHeight<0)
        {
            std::cout<<"fail measure object height\n"<<std::flush;
            return false;
        }

        std::cout<<"objectHeight="<<objectHeight<<"\n"<<std::flush;

        cv::Vec3f fpos[2];
        double objectWidth=URControlDepthCameraA::GetFingerPosition(fpos[0], fpos[1], objectOnFrame, planeAtCameraf, objectHeight, fingerThickmm/1000+0.01);

        cv::Vec3d fingerPosBase[2];

        fingerPosBase[0]=HeadToBase(cv::Vec3d(fpos[0][0], fpos[0][1], fpos[0][2]), depthCameraToBase);
        fingerPosBase[1]=HeadToBase(cv::Vec3d(fpos[1][0], fpos[1][1], fpos[1][2]), depthCameraToBase);

        double objectWidthmm=objectWidth*1000;



        openmm=objectWidthmm+20;

        double zOffsetmm=RG2ZOffset(objectWidthmm/2);


        fingerToHead[2]=fingerToHead[2]+0.01+objectHeight/2;

        cv::Vec6d pose=PickBottle(fingerPosBase[0], fingerPosBase[1], fingerToHead, planeAtBase);

        SetTCPPose(ipUR, ipPC, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], a, v);



        fingerToHead[2]=fingerToHead[2]-0.01-objectHeight/2;

        fingerToHead[2]=fingerToHead[2]-(zOffsetmm+fingerThickmm/2)/1000;

        pose=PickBottle(fingerPosBase[0], fingerPosBase[1], fingerToHead, planeAtBase);

        RG2(ipUR, ipPC, openmm);


        SetTCPPose(ipUR, ipPC, pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], a, v);


        RG2(ipUR, ipPC, 0);



        cv::Vec6d urp=cv::Vec6d(-0.151424,-0.270796,0.43873,-1.30697,2.84104,-0.01504);

        URControl::SetTCPPose(ipUR, ipPC, urp[0],urp[1],urp[2],urp[3],urp[4],urp[5],a,v);




//        SetTCPPose(ipUR, ipPC, headToBase[0], headToBase[1], headToBase[2], headToBase[3], headToBase[4], headToBase[5], a, v);


//        RG2(ipUR, ipPC, openmm);

//        RG2(ipUR, ipPC, 0);

        return true;
    }




#if 1
    virtual void ProcessFrame(const cv::Mat &color, const cv::Mat &ir, const cv::Mat &depth, const cv::Mat &xyz, const cv::Mat &texture, const cv::Mat &aligned_depth, const cv::Mat &aligned_xyz)
    {


        if(k=='t')
        {
            cv::Vec4f planeAtCamera=GetTablePlane(aligned_xyz);

            std::cout<<"calib plane\n"<<std::flush;

            char ipUR[]="192.168.0.200";
            char ipPC[]="192.168.0.202";


            cv::Vec6d headToBase;
            GetTCPPose(ipUR, ipPC, headToBase[0], headToBase[1], headToBase[2], headToBase[3], headToBase[4], headToBase[5]);

            cv::Vec6d depthCameraToBase=ComposeTransform(depthCameraToHead, headToBase);

            cv::Vec4d planeAtBase=HeadToBase(cv::Vec4d(planeAtCamera[0], planeAtCamera[1], planeAtCamera[2], planeAtCamera[3]), depthCameraToBase);

            std::cout<<"planeAtBase="<<planeAtBase<<"\n"<<std::flush;
        }

        if(k=='c')
        {
            cv::RotatedRect rr=SelectObjectRect(color, 320, 240);




            char ipUR[]="192.168.0.200";
            char ipPC[]="192.168.0.202";

            cv::Vec4d planeAtBase(0.0131388, 0.021942, -0.999673, 0.0213691);

            cv::Vec6d fingerToHead(0,0,0.21,0,0,CV_PI/4);

            double openmm;
            PickBox(openmm, planeAtBase, fingerToHead, aligned_xyz, rr, ipUR, ipPC, 0.02, 0.2);
        }


        // Display in a GUI
        cv::namedWindow("color Image", cv::WINDOW_AUTOSIZE );
        cv::imshow("color Image", color);
        cv::namedWindow("aligned depth Image ColorMap", cv::WINDOW_AUTOSIZE );
        cv::imshow("aligned depth Image ColorMap", ColorMap(aligned_depth));
        k=cv::waitKey(1);
    }
#endif



};

#endif // URCONTROLDEPTHCAMERAA_HPP
